#include "SpirateBot.h"

// 设置数据结构体
typedef struct struct_message {
  int left_1;
  int left_2;
  int right_1;
  int right_2;
  int btn_1;
  int btn_2;

} struct_message;

struct_message cmdData;


long duration;
float distanceCm;

bool isSend = false;
int value = 5;
int lastValue = 5;

int btn_value_1 = 0;
int btn_lastValue_1 = 0;

int btn_value_2 = 0;
int btn_lastValue_2 = 0;

void SpirateBot::InitESPNow() {
  WiFi.disconnect();
  if (esp_now_init() == ESP_OK) {
    Serial.println("ESPNow Init Success");
  } else {
    Serial.println("ESPNow Init Failed");
    ESP.restart();
  }
}

void SpirateBot::configDeviceAP() {
  const char *SSID = "Slave_1";
  bool result = WiFi.softAP(SSID, "Slave_1_Password", CHANNEL, 0);
  if (!result) {
    Serial.println("AP Config failed.");
  } else {
    Serial.println("AP Config Success. Broadcasting with AP: " + String(SSID));
    Serial.print("AP CHANNEL ");
    Serial.println(WiFi.channel());
  }
}
void SpirateBot::front() {
  Serial2.print(1);  //发送执行
}
void SpirateBot::back() {
  Serial2.print(2);  //发送执行
}
void SpirateBot::left() {
  Serial2.print(3);  //发送执行
}
void SpirateBot::right() {
  Serial2.print(4);  //发送执行
}
void SpirateBot::stop() {
  Serial2.print(5);  //发送执行
}
void SpirateBot::jump() {
  Serial2.print(7);  //发送执行
}
void SpirateBot::pushUp() {
  Serial2.print(6);  //发送执行
}
// callback when data is recv from Master
void OnDataRecv(const uint8_t *mac_addr, const uint8_t *data, int data_len) {

  memcpy(&cmdData, data, data_len);
  btn_value_1 = cmdData.btn_1;
  btn_value_2 = cmdData.btn_2;
  if (cmdData.left_2 == 0) {  //前进
    value = 1;
  } else if (cmdData.left_2 == 4095) {  //后退
    value = 2;
  } else if (cmdData.left_1 == 0) {  //右转
    value = 4;
  } else if (cmdData.left_1 == 4095) {  //左转
    value = 3;
  } else if ((cmdData.left_1 > 1900 && cmdData.left_1 < 2000) || (cmdData.left_2 > 1900 && cmdData.left_2 < 2000) || (cmdData.left_2 > 1900 && cmdData.left_2 < 2000) || (cmdData.right_1 > 1900 && cmdData.right_1 < 2000) || (cmdData.right_2 > 1900 && cmdData.right_2 < 2000)) {  //停止
    value = 5;
  }
  if (lastValue != value) {
    Serial2.print(value);  //发送执行
    lastValue = value;
  }

  if (btn_value_1 != btn_lastValue_1) {

    Serial2.print(6);
    btn_lastValue_1 = btn_value_1;
  }

  if (btn_value_2 != btn_lastValue_2) {

    Serial2.print(7);
    btn_lastValue_2 = btn_value_2;
  }
}
int lastDistance = 0;
float distance;
TaskHandle_t task1Handle = NULL;
const int trigPin = 18;
const int echoPin = 19;
void checkSensors(void *parameter) {
  for (;;) {  // infinite loop
    digitalWrite(trigPin, LOW);
    delayMicroseconds(2 / portTICK_PERIOD_MS);
    digitalWrite(trigPin, HIGH);
    delayMicroseconds(10 / portTICK_PERIOD_MS);
    digitalWrite(trigPin, LOW);
    duration = pulseIn(echoPin, HIGH);
    float distanceCm = duration * SOUND_SPEED / 2;
    if (distanceCm > 4) {
      distance = distanceCm;
    } else {
      Serial.println("**********************");
    }
    // Pause the task for 200ms
    vTaskDelay(200 / portTICK_PERIOD_MS);
  }
}
int SpirateBot::getDistance() {
  return distance;
}

void stopTask(void *parameter) {
  if (task1Handle != NULL) {
    vTaskDelete(task1Handle);
  }
}


void SpirateBot::startSensor() {
  xTaskCreate(
    checkSensors,    // Function that should be called
    "checkSensors",  // Name of the task (for debugging)
    5000,            // Stack size (bytes)
    NULL,            // Parameter to pass
    1,               // Task priority
    &task1Handle     // Task handle
  );
}


void SpirateBot::enableRCController() {

  WiFi.mode(WIFI_AP);
  configDeviceAP();
  Serial.print("AP MAC: ");
  Serial.println(WiFi.softAPmacAddress());
  InitESPNow();
  esp_now_register_recv_cb(OnDataRecv);
}
void SpirateBot::Init() {

  Serial.begin(115200);  
  Serial2.begin(115200);  

  pinMode(trigPin, OUTPUT);
  pinMode(echoPin, INPUT);

  startSensor(); 
}
